#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>

#include "cv.h"
#include "highgui.h"

//#include <opencv.hpp>



//#include "ros/ros.h"

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>



//#include <sensor_msgs/PointCloud2.h>
//#include <std_msgs/Int32.h>

#include <Eigen/Core> 
#include <Eigen/Geometry>



#include "graph/VertexCamera.cpp"
#include "graph/VertexPlane.cpp"
#include "graph/VertexPoint3D.cpp"
#include "graph/EdgeCameraPlane.cpp"



#include "FeatureDescriptor.cpp"
#include "SurfFeatureDescriptor64.cpp"
//#include "Histogram.cpp"
#include "RGBFeatureDescriptor.cpp"
#include "OrbFeatureDescriptor.cpp"



#include "Point.cpp"
#include "Plane.cpp"
#include "Line.cpp"

#include "Frame_input.h"
//#include "cvap_6D_slam/kinect_data.h" // kinect_data msg

#include <string.h>
#include "KeyPoint.cpp"
#include "KeyPointSet.cpp"

#include "FeatureExtractor.cpp"
#include "SurfExtractor.cpp"
#include "EdgeDetector.cpp"
//#include "segmentation.cpp"

#include "Map3D.cpp"
#include "RGBDFrame.cpp"
//#include "Transformation.cpp"

#include "Association.cpp"
#include "KeyPointAssociation.cpp"
#include "PlaneAssociation.cpp"
#include "AssociationSet.cpp"

#include "FrameMatcher.cpp"
#include "DummyMatcher.cpp"
#include "SurfRansacMatcherI.cpp"
#include "JCMergeMatcherI.cpp"
#include "JCSearchMatcherI.cpp"
#include "BasicIcpMatcher.cpp"
#include "BasicGIcpMatcher.cpp"
#include "KeyPointIcpMatcherI.cpp"
#include "AICK.cpp"
#include "AICK2.cpp"
//#include "Map.cpp"

#include <pthread.h>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>




//cvap_6D_slam::Map * mymap;


using namespace std;

struct groundtruth {
	double timestamp;
	double tx;
	double ty;
	double tz;
	double qx;
	double qy;
	double qz;
	double qw;
};

struct timestamp_path {
	double timestamp;
	string path;
};

struct frame_data {
	timestamp_path * rgb;
	timestamp_path * depth;
	groundtruth * gt;
	RGBDFrame * frame;
};

struct Analyzation_data {
	float val_time;
	float pos_error;
	float trans_error;
	RGBDFrame * src;
	RGBDFrame * dst;
	Transformation * transformation;
};

struct test_task {
	frame_data * src;
	frame_data * dst;
	FrameMatcher * matcher;
	int matcher_id;
	string filename;
	Analyzation_data * result;
};

pthread_mutex_t tasks_mutex = PTHREAD_MUTEX_INITIALIZER;
vector<test_task * > * tasks;

pthread_mutex_t done_tasks_mutex = PTHREAD_MUTEX_INITIALIZER;
int done_tasks = 0;

void printImgData(IplImage * img)
{
	printf("nSize = %i ",img->nSize);
	printf("ID = %i ",img->ID);
	printf("nChannels = %i ",img->nChannels);
	printf("alphaChannel = %i ",img->alphaChannel);
	printf("depth = %i ",img->depth);
	printf("dataOrder = %i ",img->dataOrder);
	printf("origin = %i ",img->origin);
	printf("align = %i ",img->align);
	printf("widthStep = %i ",img->widthStep);
	printf("width = %i ",img->width);
	printf("height = %i\n",img->height);
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr generatePC(frame_data * fd, int calibration)
{
	IplImage* rgb_img 	= cvLoadImage(fd->rgb->path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
	char * rgb_data		= rgb_img->imageData;
	IplImage* depth_img = cvLoadImage(fd->depth->path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
	short * depth_data	= (short *)depth_img->imageData;
	
	IplImage * test_img 	= cvCreateImage(cvSize(rgb_img->width, rgb_img->height), IPL_DEPTH_32F, 3);
	//printImgData(test_img);
	//float * test_data		= (float *)(test_img->imageData);
	float focalLengthX;
	float focalLengthY;
	float centerX;
	float centerY;
	float scalingFactor = 5000.0;
	float depthScaling;

	if(calibration == 0){
		focalLengthX = 525.0;
		focalLengthY = 525.0;
		centerX = 319.5;
		centerY = 239.5;
		depthScaling = 1;
	}else if(calibration == 1){
	 	 	 	
		focalLengthX = 517.3f;
		focalLengthY = 516.5f;
		centerX = 318.6f;
		centerY = 255.3f;
		depthScaling = 1.035f;
	}

	float invScaling = 1/scalingFactor;
	float invFocalX = 1/focalLengthX;
	float invFocalY = 1/focalLengthY;
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud->width    = rgb_img->width;
	cloud->height   = rgb_img->height;
	//cloud.is_dense = false;
	cloud->points.resize (cloud->width * cloud->height);
	int point_index = 0;
	//exit(0);
	for(int j = 0; j < rgb_img->height; j++){
		for(int i = 0; i < rgb_img->width; i++){
			int ind_rgb   = j * (8*rgb_img->widthStep/rgb_img->depth/rgb_img->nChannels) + i;
			//int ind_depth = j * (8*rgb_img->widthStep/rgb_img->depth/depth_img->nChannels) + i;
			int ind_test  = j * (8*test_img->widthStep/test_img->depth/test_img->nChannels) + i;
			//float current_depth = float(depth_data[2*ind]);
			//printf("(8*rgb_img->widthStep/rgb_img->depth/rgb_img->nChannels) = %i\n",(8*rgb_img->widthStep/rgb_img->depth/rgb_img->nChannels));
			//printf("(8*test_img->widthStep/test_img->depth/test_img->nChannels)= %i\n",(8*test_img->widthStep/test_img->depth/test_img->nChannels));
			int r = char(rgb_data[3*ind_rgb+2]);
			int g = char(rgb_data[3*ind_rgb+1]);
			int b = char(rgb_data[3*ind_rgb+0]);
			int d = short(depth_data[1*ind_test+0]);
			if(r < 0){r = 255+r;}
			if(g < 0){g = 255+g;}
			if(b < 0){b = 255+b;}
			float fr = float(r);
			float fg = float(g);
			float fb = float(b);
			float fd = float(d);
			
			float x = 0;
			float y = 0;
			float z = fd * invScaling*depthScaling;

			if(z > 0){
				x = (i - centerX) * z * invFocalX;
            	y = (j - centerY) * z * invFocalY;
			}
			cloud->points[point_index].x = x;
			cloud->points[point_index].y = y;
			cloud->points[point_index].z = z;
			cloud->points[point_index].r = fr;
			cloud->points[point_index].g = fg;
			cloud->points[point_index].b = fb;
			
			point_index++;
		}
	}
	//printf("rgb   w:%i h%i\n",rgb_img->width,rgb_img->height);
	//printf("Depth w:%i h%i\n",depth_img->width,depth_img->height);
	//cvShowImage("RGB",   rgb_img);
	//cvShowImage("Depth", depth_img);
	//cvShowImage("test",  test_img);
	//cvWaitKey(100);
	cvReleaseImage( &rgb_img );
	cvReleaseImage( &depth_img );
	cvReleaseImage( &test_img );
	return cloud;
}

void print_gt(groundtruth * gt)
{
	printf("-------------------------------------------\n");
	printf("timestamp: %f\n",gt->timestamp);
	printf("tx: %f ",gt->tx);
	printf("ty: %f ",gt->ty);
	printf("tz: %f\n",gt->tz);
	printf("qx: %f ",gt->qx);
	printf("qy: %f ",gt->qy);
	printf("qz: %f ",gt->qz);
	printf("qw: %f\n",gt->qw);
}

void print_tp(timestamp_path * tp)
{
	printf("-------------------------------------------\n");
	printf("timestamp: %f\n",tp->timestamp);
	printf("path: %s\n",tp->path.c_str());
}

void summarize_data(vector< Analyzation_data * > vec)
{
	float total_time = 0;
	int total_good = 0;
	float total_good_pos_error = 0;
	float total_good_trans_error = 0;
	int total_bad = 0;
	int total_negative = 0;
	for(unsigned int i = 0; i < vec.size(); i++)
	{
		total_time+=vec.at(i)->val_time;
		if(vec.at(i)->transformation->weight >= 10){
			if(vec.at(i)->pos_error < 0.03f && vec.at(i)->trans_error < 0.1f){
				total_good++;
				total_good_pos_error += vec.at(i)->pos_error;
				total_good_trans_error += vec.at(i)->trans_error;
			}
			else{total_bad++;}
		}else{total_negative++;}
	}
	printf("avg estimation time:        %f\n",total_time/float(vec.size()));
	printf("total good:                 %i\n",total_good);
	printf("avg possition error:        %f\n",total_good_pos_error/float(total_good));
	printf("avg transformation error:   %f\n",total_good_trans_error/float(total_good));
	printf("total bad:                  %i\n",total_bad);
	printf("total negative:             %i\n",total_negative);
}
string out_path = "output_AICK/";
//string out_path = "output/";
//string out_path = "/media/2A98195B9819273B/output_ssd/output/output/";
//string out_path = "/media/2A98195B9819273B/output_ssd/test/";
	
void test_a_task(test_task * t)
{
		frame_data * src_fd = t->src;
		frame_data * dst_fd = t->dst;
		
		struct timeval start, end;
		gettimeofday(&start, NULL);
		Transformation * trans = t->matcher->getTransformation(src_fd->frame, dst_fd->frame);
		gettimeofday(&end, NULL);
		
		float eval_time = (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f;

		g2o::SE3Quat src_poseSE3 (Eigen::Quaterniond(src_fd->gt->qw, src_fd->gt->qx, src_fd->gt->qy, src_fd->gt->qz), Eigen::Vector3d(src_fd->gt->tx, src_fd->gt->ty, src_fd->gt->tz));
		g2o::SE3Quat dst_poseSE3 (Eigen::Quaterniond(dst_fd->gt->qw, dst_fd->gt->qx, dst_fd->gt->qy, dst_fd->gt->qz), Eigen::Vector3d(dst_fd->gt->tx, dst_fd->gt->ty, dst_fd->gt->tz));
				
		Eigen::Matrix4f src_mat = src_poseSE3.to_homogenious_matrix().cast<float>();
		Eigen::Matrix4f dst_mat = dst_poseSE3.to_homogenious_matrix().cast<float>();
				
		Eigen::Matrix4f true_trans_mat = dst_mat.inverse()*src_mat;
		if(trans == NULL){
			trans = new Transformation();
			trans->src = src_fd->frame;
			trans->dst = dst_fd->frame;
			trans->transformationMatrix = true_trans_mat;
			trans->weight = 100000;
		}
		float dx = trans->transformationMatrix(0,3)-true_trans_mat(0,3);
		float dy = trans->transformationMatrix(1,3)-true_trans_mat(1,3);
		float dz = trans->transformationMatrix(2,3)-true_trans_mat(2,3);
		float pos_diff = sqrt(dx*dx+dy*dy+dz*dz);
		float trans_diff = 0;
		for(int q = 0; q < 3; q++)
		{
			for(int l = 0; l < 3; l++)
			{
				trans_diff+= fabs(trans->transformationMatrix(q,l)-true_trans_mat(q,l));
			}
		}
		Analyzation_data * data = new Analyzation_data();
		//printf("----------------------\n");
		//printf("eval_time:%f\n",eval_time);
		data->val_time			= eval_time;
		data->pos_error			= pos_diff;
		data->trans_error		= trans_diff;
		data->src 				= src_fd->frame;
		data->dst 				= dst_fd->frame;
		data->transformation	= trans;
		t->result = data;
}

int nr_done_tasks(){
    pthread_mutex_lock(&done_tasks_mutex);
	int val = done_tasks;
	pthread_mutex_unlock(&done_tasks_mutex);
	return val;
}

void * start_test_thread( void *ptr )
{
	while(true)
	{
		pthread_mutex_lock( &tasks_mutex );
		if(tasks->size() > 0)
		{
			test_task * t = tasks->back();
			tasks->pop_back();
			pthread_mutex_unlock(&tasks_mutex);
			
			test_a_task(t);

			long size 				= sizeof(int)*(8);
			char * buffer_char 		= new char[size];
			int * buffer_int 		= (int *)buffer_char;
			float * buffer_float	= (float *)buffer_char;
			string path = out_path+t->filename;
			ifstream task_file (path.c_str());
			if (task_file.is_open()){
				task_file.seekg(0,ifstream::end);
				long size=task_file.tellg();
				char * buffer_char 		= new char [size];
				int * buffer_int 		= (int *) buffer_char;
				float * buffer_float	= (float *) buffer_char;
		
				task_file.seekg(0);
				task_file.read (buffer_char,size);
				task_file.close();
				buffer_int[3] = 1;
				buffer_float[4] = t->result->val_time;
				buffer_float[5] = t->result->pos_error;
				buffer_float[6] = t->result->trans_error;
				buffer_float[7] = t->result->transformation->weight;
				
				buffer_float[8 ] = t->result->transformation->transformationMatrix(0,0);
				buffer_float[9 ] = t->result->transformation->transformationMatrix(0,1);
				buffer_float[10] = t->result->transformation->transformationMatrix(0,2);
				buffer_float[11] = t->result->transformation->transformationMatrix(0,3);
				buffer_float[12] = t->result->transformation->transformationMatrix(1,0);
				buffer_float[13] = t->result->transformation->transformationMatrix(1,1);
				buffer_float[14] = t->result->transformation->transformationMatrix(1,2);
				buffer_float[15] = t->result->transformation->transformationMatrix(1,3);
				buffer_float[16] = t->result->transformation->transformationMatrix(2,0);
				buffer_float[17] = t->result->transformation->transformationMatrix(2,1);
				buffer_float[18] = t->result->transformation->transformationMatrix(2,2);
				buffer_float[19] = t->result->transformation->transformationMatrix(2,3);
				buffer_float[20] = t->result->transformation->transformationMatrix(3,0);
				buffer_float[21] = t->result->transformation->transformationMatrix(3,1);
				buffer_float[22] = t->result->transformation->transformationMatrix(3,2);
				buffer_float[23] = t->result->transformation->transformationMatrix(3,3);
				
				ofstream outfile (path.c_str(),ofstream::binary);
				outfile.write (buffer_char,size);
				outfile.close();
				delete buffer_char;
				
				pthread_mutex_lock(&done_tasks_mutex);
				done_tasks++;
				pthread_mutex_unlock(&done_tasks_mutex);
			}
			
		}else{
			pthread_mutex_unlock(&tasks_mutex);
			usleep(1000);
		}
	}
}

int getdir (string dir, vector<string> &files)
{
    DIR *dp;
    struct dirent *dirp;
    if((dp  = opendir(dir.c_str())) == NULL) {
        cout << "Error(" << errno << ") opening " << dir << endl;
        return errno;
    }

    while ((dirp = readdir(dp)) != NULL) {
        files.push_back(string(dirp->d_name));
    }
    closedir(dp);
    return 0;
}

bool mycomparison (test_task * i,test_task * j) {
	return (i->src->frame->id - i->dst->frame->id) > (j->src->frame->id - j->dst->frame->id);
}

void test(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer)
{
	string path = "/home/johane/test_data/rgbd_dataset_freiburg1_room";

																																																																																																																																																																																																																																																																																																																																																																																	
	string path_gt = path+"/groundtruth.txt";
	string path_rgb = path+"/rgb.txt";
	string path_depth = path+"/depth.txt";
		vector<FrameMatcher * > matchers;
	vector<vector< Analyzation_data * > > matchers_data;
//exit(0);
	//matchers.push_back(new SurfRansacMatcherI(500, 0.001, 0.05, 500, 0.2f));
	//matchers_data.push_back(vector< Analyzation_data * >());
	
	//========================================
	//==============BASE TEST=================
	//========================================
	/*
	int max_backing = 5;
	string prefix = "test";
	
	matchers.push_back(new BasicIcpMatcher());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	matchers.push_back(new BasicGIcpMatcher());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	matchers.push_back(new KeyPointIcpMatcherI());
	matchers_data.push_back(vector< Analyzation_data * >());

	matchers.push_back(new DummyMatcher());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==============KeyPointIcp===============
	//========================================
	/*
	string prefix = "KeyPointIcp";
	int max_backing = 150;
	
	matchers.push_back(new KeyPointIcpMatcherI());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==============BasicGIcpMatcher===============
	//========================================
	
	/*
	string prefix = "BasicGIcpMatcher";
	int max_backing = 7;
	
	matchers.push_back(new BasicGIcpMatcher());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	
	//========================================
	//==============pathtrace===============
	//========================================
	/*
	string prefix = "pathtrace";
	int max_backing = 1;
	
	matchers.push_back(new BasicGIcpMatcher());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	matchers.push_back(new KeyPointIcpMatcherI());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	
	//========================================
	//==============pathtracev2===============
	//========================================
	/*
	string prefix = "pathtracev2";
	int max_backing = 1;
	
	matchers.push_back(new BasicGIcpMatcher());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	matchers.push_back(new KeyPointIcpMatcherI());
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==============groundtrouth=============
	//========================================
	/*
	string prefix = "groundtrouth";
	int max_backing = 1;
	
	DummyMatcher * d = new DummyMatcher();
	d->mode = 1;
	matchers.push_back(d);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==============KeyPointIcpStrong=============
	//========================================
	/*
	string prefix = "KeyPointIcpStrong";
	int max_backing = 1;
	
	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	matchers.push_back(m);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==============KeyPointIcpStrongv2=============
	//========================================
	
	/*
	string prefix = "KeyPointIcpStrongv2";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	m->stabilety_threshold = 0.1f*0.001f;
	matchers.push_back(m);
	matchers_data.push_back(vector< Analyzation_data * >());
	int calibration = 0;
	*/
	//========================================
	//==========KeyPointIcpStrongv2calb1============
	//========================================
	/*
	string prefix = "KeyPointIcpStrongv2calb1";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	m->stabilety_threshold = 0.1f*0.001f;
	matchers.push_back(m);
	matchers_data.push_back(vector< Analyzation_data * >());
	int calibration = 1;
	*/

	//========================================
	//==========calib1test============
	//========================================
	/*
	string prefix = "calib1test";
	int max_backing = 1;
	
	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	m->stabilety_threshold = 0.1f*0.001f;
	matchers.push_back(m);
	matchers_data.push_back(vector< Analyzation_data * >());
	int calibration = 1;
	*/
	
	//========================================
	//==========calib0test============
	//========================================
	/*
	string prefix = "calib0test";
	int max_backing = 1;
	
	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	m->stabilety_threshold = 0.1f*0.001f;
	matchers.push_back(m);
	matchers_data.push_back(vector< Analyzation_data * >());
	int calibration = 0;
	*/
	
	//========================================
	//==========NrKPs============
	//========================================
	/*
	string prefix = "NrKPs";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m1 = new KeyPointIcpMatcherI();
	m1->nr_iter = 150;
	m1->shrinking = 0.95f;
	m1->max_points = 500;
	matchers.push_back(m1);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m2 = new KeyPointIcpMatcherI();
	m2->nr_iter = 150;
	m2->shrinking = 0.95f;
	m2->max_points = 400;
	matchers.push_back(m2);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m3 = new KeyPointIcpMatcherI();
	m3->nr_iter = 150;
	m3->shrinking = 0.95f;
	m3->max_points = 300;
	matchers.push_back(m3);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m4 = new KeyPointIcpMatcherI();
	m4->nr_iter = 150;
	m4->shrinking = 0.95f;
	m4->max_points = 200;
	matchers.push_back(m4);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m5 = new KeyPointIcpMatcherI();
	m5->nr_iter = 150;
	m5->shrinking = 0.95f;
	m5->max_points = 100;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m6 = new KeyPointIcpMatcherI();
	m6->nr_iter = 150;
	m6->shrinking = 0.95f;
	m6->max_points = 50;
	matchers.push_back(m6);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m7 = new KeyPointIcpMatcherI();
	m7->nr_iter = 150;
	m7->shrinking = 0.95f;
	m7->max_points = 25;
	matchers.push_back(m7);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m8 = new KeyPointIcpMatcherI();
	m8->nr_iter = 150;
	m8->shrinking = 0.95f;
	m8->max_points = 10;
	matchers.push_back(m8);
	matchers_data.push_back(vector< Analyzation_data * >());
	int calibration = 0;
	*/
	//========================================
	//==========NrIter============
	//========================================
	/*
	string prefix = "NrIter";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m0 = new KeyPointIcpMatcherI();
	m0->nr_iter = 100;
	m0->shrinking = 0.95f;
	m0->max_points = 200;
	matchers.push_back(m0);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m1 = new KeyPointIcpMatcherI();
	m1->nr_iter = 50;
	m1->shrinking = 0.9f;
	m1->max_points = 200;
	matchers.push_back(m1);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m2 = new KeyPointIcpMatcherI();
	m2->nr_iter = 25;
	m2->shrinking = 0.85f;
	m2->max_points = 200;
	matchers.push_back(m2);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m3 = new KeyPointIcpMatcherI();
	m3->nr_iter = 10;
	m3->shrinking = 0.7f;
	m3->max_points = 200;
	matchers.push_back(m3);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m4 = new KeyPointIcpMatcherI();
	m4->nr_iter = 5;
	m4->shrinking = 0.5f;
	m4->max_points = 200;
	matchers.push_back(m4);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m5 = new KeyPointIcpMatcherI();
	m5->nr_iter = 3;
	m5->shrinking = 0.3f;
	m5->max_points = 200;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m6 = new KeyPointIcpMatcherI();
	m6->nr_iter = 2;
	m6->shrinking = 0.8f;
	m6->max_points = 200;
	matchers.push_back(m6);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m7 = new KeyPointIcpMatcherI();
	m7->nr_iter = 1;
	m7->shrinking = 0.95f;
	m7->max_points = 200;
	matchers.push_back(m7);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	
	//========================================
	//==========kpICP1============
	//========================================
	/*
	string prefix = "kpICP1";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m5 = new KeyPointIcpMatcherI();
	m5->nr_iter = 3;
	m5->shrinking = 0.3f;
	m5->max_points = 200;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==========AICK1============
	//========================================
	/*
	string prefix = "AICK1";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m5 = new KeyPointIcpMatcherI();
	m5->nr_iter = 5;
	m5->shrinking = 0.5f;
	m5->max_points = 200;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	int calibration = 0;
	*/
	//========================================
	//==========AICK2============
	//========================================
	/*
	string prefix = "AICK2";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m5 = new KeyPointIcpMatcherI();
	m5->nr_iter = 5;
	m5->shrinking = 0.5f;
	m5->max_points = 300;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m4 = new KeyPointIcpMatcherI();
	m4->nr_iter = 5;
	m4->shrinking = 0.5f;
	m4->max_points = 400;
	matchers.push_back(m4);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m6 = new KeyPointIcpMatcherI();
	m5->nr_iter = 5;
	m5->shrinking = 0.5f;
	m5->max_points = 500;
	matchers.push_back(m6);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m7 = new KeyPointIcpMatcherI();
	m7->nr_iter = 5;
	m7->shrinking = 0.5f;
	m7->max_points = 600;
	matchers.push_back(m7);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m8 = new KeyPointIcpMatcherI();
	m8->nr_iter = 25;
	m8->shrinking = 0.7f;
	m8->max_points = 8000;
	matchers.push_back(m8);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	KeyPointIcpMatcherI * m9 = new KeyPointIcpMatcherI();
	m9->nr_iter = 150;
	m9->shrinking = 0.85f;
	m9->max_points = 8000;
	matchers.push_back(m9);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	
	//========================================
	//==========AICKtest6============
	//========================================
	/*
	string prefix = "AICKtest6";
	int max_backing = 30;
	
	AICK * m0= new AICK();
	m0->nr_iter = 10;
	m0->shrinking = 0.5f;
	m0->max_points = 200;
	matchers.push_back(m0);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK2 * m1= new AICK2();
	m1->nr_iter = 10;
	m1->shrinking = 0.5f;
	m1->max_points = 200;
	matchers.push_back(m1);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==========AICKFULL============
	//========================================
	/*
	string prefix = "AICKFULL";
	int max_backing = 30;
	
	AICK * m0= new AICK();//Full
	m0->nr_iter = 150;
	m0->shrinking = 0.95f;
	m0->max_points = 200000;
	matchers.push_back(m0);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m1= new AICK();//Full(max 800)
	m1->nr_iter = 150;
	m1->shrinking = 0.95f;
	m1->max_points = 800;
	matchers.push_back(m1);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	//nrKPS
	AICK * m2= new AICK();
	m2->nr_iter = 150;
	m2->shrinking = 0.95f;
	m2->max_points = 500;
	matchers.push_back(m2);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m3= new AICK();
	m3->nr_iter = 150;
	m3->shrinking = 0.95f;
	m3->max_points = 400;
	matchers.push_back(m3);
	matchers_data.push_back(vector< Analyzation_data * >());

	AICK * m4= new AICK();
	m4->nr_iter = 150;
	m4->shrinking = 0.95f;
	m4->max_points = 300;
	matchers.push_back(m4);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m5= new AICK();
	m5->nr_iter = 150;
	m5->shrinking = 0.95f;
	m5->max_points = 200;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m6= new AICK();
	m6->nr_iter = 150;
	m6->shrinking = 0.95f;
	m6->max_points = 100;
	matchers.push_back(m6);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m7= new AICK();
	m7->nr_iter = 150;
	m7->shrinking = 0.95f;
	m7->max_points = 50;
	matchers.push_back(m7);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	//nrIter
	AICK * m8 = new AICK();
	m8->nr_iter = 100;
	m8->shrinking = 0.95f;
	m8->max_points = 200;
	matchers.push_back(m8);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m9 = new AICK();
	m9->nr_iter = 50;
	m9->shrinking = 0.9f;
	m9->max_points = 200;
	matchers.push_back(m9);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m10 = new AICK();
	m10->nr_iter = 25;
	m10->shrinking = 0.85f;
	m10->max_points = 200;
	matchers.push_back(m10);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m11 = new AICK();
	m11->nr_iter = 10;
	m11->shrinking = 0.7f;
	m11->max_points = 200;
	matchers.push_back(m11);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m12 = new AICK();
	m12->nr_iter = 5;
	m12->shrinking = 0.5f;
	m12->max_points = 200;
	matchers.push_back(m12);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m13 = new AICK();
	m13->nr_iter = 3;
	m13->shrinking = 0.3f;
	m13->max_points = 200;
	matchers.push_back(m13);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m14 = new AICK();
	m14->nr_iter = 2;
	m14->shrinking = 0.2f;
	m14->max_points = 200;
	matchers.push_back(m14);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m15 = new AICK();
	m15->nr_iter = 1;
	m15->shrinking = 0.1f;
	m15->max_points = 200;
	matchers.push_back(m15);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==========AICKFULLv2============
	//========================================
	
	string prefix = "AICKFULLv2";
	int max_backing = 30;
	
	AICK * m0= new AICK();//Full
	m0->nr_iter = 150;
	m0->shrinking = 0.95f;
	m0->max_points = 200000;
	matchers.push_back(m0);
	matchers_data.push_back(vector< Analyzation_data * >());

	AICK * m12 = new AICK();
	m12->nr_iter = 5;
	m12->shrinking = 0.5f;
	m12->max_points = 200;
	matchers.push_back(m12);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	//========================================
	//==========AICK4============
	//========================================
	/*
	string prefix = "AICK4";
	int max_backing = 30;
	
	AICK * m0= new AICK();
	m0->nr_iter = 150;
	m0->shrinking = 0.95f;
	m0->max_points = 8000;
	matchers.push_back(m0);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m1 = new AICK();
	m1->nr_iter = 100;
	m1->shrinking = 0.9f;
	m1->max_points = 8000;
	matchers.push_back(m1);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m2 = new AICK();
	m2->nr_iter = 50;
	m2->shrinking = 0.8f;
	m2->max_points = 8000;
	matchers.push_back(m2);
	matchers_data.push_back(vector< Analyzation_data * >());

	AICK * m3 = new AICK();
	m3->nr_iter = 50;
	m3->shrinking = 0.7f;
	m3->max_points = 8000;
	matchers.push_back(m3);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m4 = new AICK();
	m4->nr_iter = 25;
	m4->shrinking = 0.6f;
	m4->max_points = 8000;
	matchers.push_back(m4);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m5 = new AICK();
	m5->nr_iter = 10;
	m5->shrinking = 0.5f;
	m5->max_points = 8000;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m6 = new AICK();
	m6->nr_iter = 5;
	m6->shrinking = 0.4f;
	m6->max_points = 8000;
	matchers.push_back(m6);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m7 = new AICK();
	m7->nr_iter = 3;
	m7->shrinking = 0.3f;
	m7->max_points = 8000;
	matchers.push_back(m7);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m8 = new AICK();
	m8->nr_iter = 2;
	m8->shrinking = 0.2f;
	m8->max_points = 8000;
	matchers.push_back(m8);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	AICK * m9 = new AICK();
	m9->nr_iter = 1;
	m9->shrinking = 0.2f;
	m9->max_points = 8000;
	matchers.push_back(m9);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	//========================================
	//==========kpICP2============
	//========================================
	/*
	string prefix = "kpICP2";
	int max_backing = 30;
	
	KeyPointIcpMatcherI * m5 = new KeyPointIcpMatcherI();
	m5->nr_iter = 25;
	m5->shrinking = 0.9f;
	m5->max_points = 200000;
	matchers.push_back(m5);
	matchers_data.push_back(vector< Analyzation_data * >());
	
	int calibration = 0;
	*/
	printf("starting setup\n");
	//===============
	//===============
	
	
	string line;
	ifstream gt_file (path_gt.c_str());
	vector< groundtruth * > gt_vec;
	if (gt_file.is_open()){
		while ( gt_file.good() )
		{
			getline (gt_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					
					int space2 = line.find(" ",space1+1,1);
					int space3 = line.find(" ",space2+1,1);
					int space4 = line.find(" ",space3+1,1);
					int space5 = line.find(" ",space4+1,1);
					int space6 = line.find(" ",space5+1,1);
					int space7 = line.find(" ",space6+1,1);
					
					groundtruth * gt = new groundtruth();
					gt->timestamp 	= atof(line.substr(0,space1).c_str());
					gt->tx 			= atof(line.substr(space1+1,space2-space1-1).c_str());
					gt->ty 			= atof(line.substr(space2+1,space3-space2-1).c_str());
					gt->tz 			= atof(line.substr(space3+1,space4-space3-1).c_str());
					gt->qx 			= atof(line.substr(space4+1,space5-space4-1).c_str());
					gt->qy 			= atof(line.substr(space5+1,space6-space5-1).c_str());
					gt->qz 			= atof(line.substr(space6+1,space7-space6-1).c_str());
					gt->qw 			= atof(line.substr(space7+1,line.length()-space7-1).c_str());
					gt_vec.push_back(gt);
				}
			}
		}
		gt_file.close();
	}else{cout << "Unable to open " << path_gt;}
	
	ifstream rgb_file (path_rgb.c_str());
	vector< timestamp_path * > rgb_vec;
	if (rgb_file.is_open()){
		while ( rgb_file.good() )
		{
			getline (rgb_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					timestamp_path * tp = new timestamp_path();
					tp->timestamp 		= atof(line.substr(0,space1).c_str());
					tp->path			= path+"/"+line.substr(space1+1,line.length()-space1-1);
					rgb_vec.push_back(tp);
					//print_tp(tp);
					//printf("Line: %s\n",line.c_str());
				}
			}
		}
		rgb_file.close();
	}else{cout << "Unable to open " << path_rgb;}
	
	ifstream depth_file (path_depth.c_str());
	vector< timestamp_path * > depth_vec;
	if (depth_file.is_open()){
		while ( depth_file.good() )
		{
			getline (depth_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					timestamp_path * tp = new timestamp_path();
					tp->timestamp 		= atof(line.substr(0,space1).c_str());
					tp->path			= path+"/"+line.substr(space1+1,line.length()-space1-1);
					depth_vec.push_back(tp);
					//print_tp(tp);
					//printf("Line: %s\n",line.c_str());
				}
			}
		}
		depth_file.close();
	}else{cout << "Unable to open " << path_depth;}
	printf("rgb_vec: %i depth_vec: %i gt_vec: %i\n",(int)rgb_vec.size(),(int)depth_vec.size(),(int)gt_vec.size());
	
	int closest_d = 0;
	int closest_t = 0;
/*
	ifstream infile ("bow/bow_test",ifstream::binary);
	infile.seekg(0,ifstream::end);
	long size=infile.tellg();
	char * buffer_char 		= new char [size];
	int * buffer_int 		= (int *) buffer_char;
	//float * buffer_float	= (float *) buffer_char;
	infile.seekg(0);
	infile.read (buffer_char,size);
	infile.close();
	int nr_centers 	= buffer_int[0];
	int desc_len	= buffer_int[1];
	int desc_type	= buffer_int[2];
*/
	vector<FeatureDescriptor *> * centers = new vector<FeatureDescriptor *>();
/*
	for(int i = 0; i < nr_centers; i++)
	{
		float * feature_desc = new float[desc_len];
		for(int j = 0; j < desc_len; j++)
		{
			FeatureDescriptor * fd 	= new FeatureDescriptor(feature_desc,desc_len);
			fd->type				= desc_type;
			centers->push_back(fd);
		}
	}
*/
	
	
	vector< frame_data * > all_frames;
	//printf("keypointcounter = [\n");
	for(int i = 0; i < rgb_vec.size() && i < 3000; i+=1)
	{
		
		printf("%i\n",i);
		float last_diff_d = 1000000;
		for(unsigned int j = closest_d; j < depth_vec.size(); j++)
		{
			float diff = fabs(rgb_vec.at(i)->timestamp - depth_vec.at(j)->timestamp);
			if(diff > last_diff_d){
				closest_d = j-1;
				break;
			}else{
				last_diff_d = diff;
			}
		}
		//printf("best_d: %i, diff_d: %f\n",closest_d,last_diff_d);
		float last_diff_t = 1000000;
		for(unsigned int j = closest_t; j < gt_vec.size(); j++)
		{
			float diff = fabs(depth_vec.at(closest_d)->timestamp - gt_vec.at(j)->timestamp);
			if(diff > last_diff_t){
				closest_t = j-1;
				break;
			}else{
				last_diff_t = diff;
				closest_t =j;
			}
		}
		//printf("best_gt: %i, diff_gt: %f\n",closest_t,last_diff_t);
		
		frame_data * fd = new frame_data();
		fd->rgb 		= rgb_vec.at(i);
		fd->depth 		= depth_vec.at(closest_d);
		fd->gt 			= gt_vec.at(closest_t);
		
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = generatePC(fd,calibration);
		pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
		pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
		ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
		ne.setMaxDepthChangeFactor(0.02f);
		ne.setNormalSmoothingSize(5.0f);
		ne.setInputCloud(cloud);
		ne.compute(*normals);
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
		pcl::concatenateFields(*cloud,*normals,*full_cloud);
		cloud.reset();
		normals.reset();
		fd->frame		= new RGBDFrame(full_cloud,centers);
		all_frames.push_back(fd);
		full_cloud.reset();
	}
	//printf("]\n");
	//exit(0);
	
	const bool debugg_testing = false;

	for(int i = 0; i < (int)all_frames.size(); i++)
	{
		for(int j = i-1; j >=  max(0,i-max_backing); j--){
			for(unsigned int k = 0; k < matchers.size(); k++)
			{
				char fpath [150];
				sprintf(fpath,"%s%s_%i_%i_%i.task",out_path.c_str(),prefix.c_str(),i,j,k);
				ifstream task_file (fpath);
				if (!task_file.is_open()){
					long size 				= sizeof(int)*(8+16);
					char * buffer_char 		= new char[size];
					int * buffer_int 		= (int *)buffer_char;
					float * buffer_float	= (float *)buffer_char;
					buffer_int[0] = i;
					buffer_int[1] = j;
					buffer_int[2] = k;
					buffer_int[3] = 0;//done?
					buffer_float[4] = 0;//val_time;
					buffer_float[5] = 0;//pos_error;
					buffer_float[6] = 0;//trans_error;
					buffer_float[7] = 0;//weight;
					
					buffer_float[8] = 0;
					buffer_float[9] = 0;
					buffer_float[10] = 0;
					buffer_float[11] = 0;
					buffer_float[12] = 0;
					buffer_float[13] = 0;
					buffer_float[14] = 0;
					buffer_float[15] = 0;
					
					buffer_float[16] = 0;
					buffer_float[17] = 0;
					buffer_float[18] = 0;
					buffer_float[19] = 0;
					buffer_float[20] = 0;
					buffer_float[21] = 0;
					buffer_float[22] = 0;
					buffer_float[23] = 0;
					
					ofstream outfile (fpath,ofstream::binary);
					outfile.write (buffer_char,size);
					outfile.close();
					delete buffer_char;
				}else{task_file.close();}
			}
		}

	}

	tasks = new vector<test_task * >();
	
	vector<string> files = vector<string>();
	getdir(out_path,files);
	int added_tasks = 0;
    for (unsigned int i = 0;i < files.size();i++) {
        if(files.at(i).find(prefix)==0 && files.at(i).find("_") == prefix.length()){
        	ifstream task_file ((out_path+files.at(i)).c_str());
			if (task_file.is_open()){
				
				task_file.seekg(0,ifstream::end);
				long size=task_file.tellg();
				char * buffer_char 		= new char [size];
				int * buffer_int 		= (int *) buffer_char;
				float * buffer_float	= (float *) buffer_char;
				task_file.seekg(0);
				task_file.read (buffer_char,size);
				task_file.close();
				
				//if(i%100 == 0){
					printf("%f---->%i %i %i %i -> %f %f %f %f\n",float(i)/float(files.size()),buffer_int[0],buffer_int[1],buffer_int[2],buffer_int[3],buffer_float[4],buffer_float[5],buffer_float[6],buffer_float[7]);
					printf("Transformation :");
					for(int k = 0; k < 16; k++){printf("%f ",buffer_float[8+k]);}
					printf("\n");
				//}
				
				if(buffer_int[3] == 0){
					//printf("creating task\n");
					frame_data * src_fd = all_frames.at(buffer_int[0]);
					frame_data * dst_fd = all_frames.at(buffer_int[1]);
					test_task * t = new test_task();
					t->src = src_fd;
					t->dst = dst_fd;
					t->matcher = matchers.at(buffer_int[2]);
					t->matcher_id = buffer_int[2];
					t->filename = string(files.at(i));
					
					pthread_mutex_lock( &tasks_mutex );
					tasks->push_back(t);
					pthread_mutex_unlock(&tasks_mutex);
					added_tasks++;
				}
				delete buffer_char;
        	}
        }
    }
    pthread_mutex_lock( &tasks_mutex );
	sort(tasks->begin(),tasks->end(),mycomparison);
	pthread_mutex_unlock(&tasks_mutex);
    for(int i = 0; i < 6; i++){
		pthread_t mythread;
		pthread_create( &mythread, NULL, start_test_thread, NULL);
	}
    
    struct timeval test_start, test_end;
	gettimeofday(&test_start, NULL);
	while(nr_done_tasks() < added_tasks){
		gettimeofday(&test_end, NULL);
		float test_time = (test_end.tv_sec*1000000+test_end.tv_usec-(test_start.tv_sec*1000000+test_start.tv_usec))/1000000.0f;
		printf("%i/%i Time spent: %f\n",nr_done_tasks(),added_tasks,test_time);
		usleep(500000);
	}

}
int main(int argc, char **argv)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	viewer->setBackgroundColor (0.5f, 0.5f, 0.5f);
	viewer->addCoordinateSystem (1.0);
	viewer->initCameraParameters ();

	test(viewer);
	return 0;
}
